/**
* @file rt_spp.cpp in RealTimeSpp
* @author rebeater
* @comment 实时SPP程序
* Create on 10/26/21 3:56 PM
* @version 1.0
**/


#include "rt_spp.h"
#include "rtklib/rtklib.h"
#include <string>


using namespace std;

extern void settspan(gtime_t ts, gtime_t te) {}
extern void settime(gtime_t time) {}
extern int showmsg(char *format, ...) {
  va_list arg;
  va_start(arg, format);
  vfprintf(stderr, format, arg);
  va_end(arg);
  fprintf(stderr, "\r");
  return 0;
}
static const char obscodes[] = "CLDS";    /* obs type codes */


void set_index(double ver, int sys, const char *opt,
			   char tobs[MAXOBSTYPE][4], sigind_t *ind) {
  const char *p;
  char str[8], *optstr = "";
  double shift;
  int i, j, k, n;

  for (i = n = 0; *tobs[i]; i++, n++) {
	ind->code[i] = obs2code(tobs[i] + 1, ind->frq + i);
	ind->type[i] = (p = strchr(obscodes, tobs[i][0])) ? (int)(p - obscodes) : 0;
	ind->pri[i] = getcodepri(sys, ind->code[i], opt);
	ind->pos[i] = -1;

	/* frequency index for beidou */
	if (sys == SYS_CMP) {
	  if (ind->frq[i] == 5) ind->frq[i] = 2; /* B2 */
	  else if (ind->frq[i] == 4) ind->frq[i] = 3; /* B3 */
	}
  }
  /* parse phase shift options */
  switch (sys) {
	case SYS_GPS: optstr = "-GL%2s=%lf";
	  break;
	case SYS_GLO: optstr = "-RL%2s=%lf";
	  break;
	case SYS_GAL: optstr = "-EL%2s=%lf";
	  break;
	case SYS_QZS: optstr = "-JL%2s=%lf";
	  break;
	case SYS_SBS: optstr = "-SL%2s=%lf";
	  break;
	case SYS_CMP: optstr = "-CL%2s=%lf";
	  break;
	case SYS_IRN: optstr = "-IL%2s=%lf";
	  break;
  }
  for (p = opt; p && (p = strchr(p, '-')); p++) {
	if (sscanf(p, optstr, str, &shift) < 2) continue;
	for (i = 0; i < n; i++) {
	  if (strcmp(code2obs(ind->code[i], NULL), str)) continue;
	  ind->shift[i] = shift;
	  trace(2, "phase shift: sys=%2d tobs=%s shift=%.3f\n", sys,
			tobs[i], shift);
	}
  }
  /* assign index for highest priority code */
  for (i = 0; i < NFREQ; i++) {
	for (j = 0, k = -1; j < n; j++) {
	  if (ind->frq[j] == i + 1 && ind->pri[j] && (k < 0 || ind->pri[j] > ind->pri[k])) {
		k = j;
	  }
	}
	if (k < 0) continue;

	for (j = 0; j < n; j++) {
	  if (ind->code[j] == ind->code[k]) ind->pos[j] = i;
	}
  }
  /* assign index of extended obs data */
  for (i = 0; i < NEXOBS; i++) {
	for (j = 0; j < n; j++) {
	  if (ind->code[j] && ind->pri[j] && ind->pos[j] < 0) break;
	}
	if (j >= n) break;

	for (k = 0; k < n; k++) {
	  if (ind->code[k] == ind->code[j]) ind->pos[k] = NFREQ + i;
	}
  }
  for (i = 0; i < n; i++) {
	if (!ind->code[i] || !ind->pri[i] || ind->pos[i] >= 0) continue;
	trace(4, "reject obs type: sys=%2d, obs=%s\n", sys, tobs[i]);
  }
  ind->n = n;

#if 0 /* for debug */
  for (i=0;i<n;i++) {
	trace(2,"set_index: sys=%2d,tobs=%s code=%2d pri=%2d frq=%d pos=%d shift=%5.2f\n",
		  sys,tobs[i],ind->code[i],ind->pri[i],ind->frq[i],ind->pos[i],
		  ind->shift[i]);
  }
#endif
}

void soltocov(const sol_t *sol, double *P) {

  P[0] = sol->qr[0]; /* xx or ee */
  P[4] = sol->qr[1]; /* yy or nn */
  P[8] = sol->qr[2]; /* zz or uu */
  P[1] = P[3] = sol->qr[3]; /* xy or en */
  P[5] = P[7] = sol->qr[4]; /* yz or nu */
  P[2] = P[6] = sol->qr[5]; /* zx or ue */

};

/**
 * 读取星历信息
 * @param nav
 * @return
 */
bool RealTimeSpp::UpdateNav(const string &nav) {
  return false;
}
/**
 * 单点定位实现
 * @param rnx
 * @return
 */
bool RealTimeSpp::Update(int week, double gpst, const char *buff) {

  int nsta;/*number of satellites*/
  int flag_;
  int *flag = &flag_;
  gtime_t time;
  char satid[8] = "";
  /* set signal index */
  double ver = 3.02;

  int i, j, m, n;
  int p[MAXOBSTYPE], k[16], l[16];
  double val[MAXOBSTYPE] = {0};
  unsigned char lli[MAXOBSTYPE] = {0};
  sigind_t *ind;


//  if ((nsta=(int)str2num(buff,32,3))<=0) return false;
  nsta = 1;
  time = gpst2time(week, gpst);

  obsds[nobs].time = time;
  strncpy(satid, buff, 3);
  obsds[nobs].sat = (unsigned char)satid2no(satid);

  /* read obs data fields */
  obsd_t *obs_cur = obsds + nobs;
  obs_cur->rcv = 1;
  switch (satsys(obs_cur->sat, NULL)) {
	case SYS_GLO: ind = index + 1;
	  break;
	case SYS_GAL: ind = index + 2;
	  break;
	case SYS_QZS: ind = index + 3;
	  break;
	case SYS_SBS: ind = index + 4;
	  break;
	case SYS_CMP: ind = index + 5;
	  break;
	default: ind = index;
	  break;
  }
  int stat = 1;
  for (i = 0, j = ver <= 2.99 ? 0 : 3; i < ind->n; i++, j += 16) {
	val[i] = str2num(buff, j, 14) + ind->shift[i];
	lli[i] = (unsigned char)str2num(buff, j + 14, 1) & 3;
  }

  for (i = 0; i < NFREQ + NEXOBS; i++) {
	obs_cur->P[i] = obs_cur->L[i] = 0.0;
	obs_cur->D[i] = 0.0f;
	obs_cur->SNR[i] = obs_cur->LLI[i] = obs_cur->code[i] = 0;
  }
  /* assign position in obs data */
  for (i = n = m = 0; i < ind->n; i++) {

	p[i] = ver <= 2.11 ? ind->frq[i] - 1 : ind->pos[i];

	if (ind->type[i] == 0 && p[i] == 0) k[n++] = i; /* C1? index */
	if (ind->type[i] == 0 && p[i] == 1) l[m++] = i; /* C2? index */
  }
  /* save obs data */
  for (i = 0; i < ind->n; i++) {
	if (p[i] < 0 || val[i] == 0.0) continue;
	switch (ind->type[i]) {
	  case 0: obs_cur->P[p[i]] = val[i];
		obs_cur->code[p[i]] = ind->code[i];
		break;
	  case 1: obs_cur->L[p[i]] = val[i];
		obs_cur->LLI[p[i]] = lli[i];
		break;
	  case 2: obs_cur->D[p[i]] = (float)val[i];
		break;
	  case 3: obs_cur->SNR[p[i]] = (unsigned char)(val[i] * 4.0 + 0.5);
		break;
	}
  }
  nobs++;
  if (nobs > MAXOBS) {
	nobs = 0;
  }
//  decode_obsepoch

  return true;
}

/**
 * Load Nav file
 * @param rnx_path
 * @return
 */
#include <iostream>
bool RealTimeSpp::LoadNavFromRnx(const string &rnx_path) {
  gtime_t ts{0, 0};
  gtime_t te{0, 0};
  double ti = 0;
  auto nav = &nav_;
  auto obs = &obs_;
  if (readrnxt(rnx_path.c_str(), 1, ts, te, ti, " ", obs, nav,
			   NULL) < 0) {
	std::cout<<(1, "insufficient memory\n");
	return false;
  }
  /* delete duplicated ephemeris */
  uniqnav(nav);
  return nav->n > 0;
}
RealTimeSpp::RealTimeSpp() {
  prcopt_ = prcopt_default;
  rtkinit(&rtk, &prcopt_);
  char opt[] = "";
  char tobs[7][MAXOBSTYPE][4] = {
	  {"C1C", "L1C", "D1C", "S1C", "C2W", "L2W", "D2W", "S2W", "", "", "", ""},/*GPS*/
	  {"C1C", "L1C", "D1C", "S1C", "C2P", "L2P", "D2P", "S2P", "", "", "", ""},/*GLO*/
	  {},/*GLA*/
	  {"C1C", "L1C", "D1C", "S1C", "C2S", "L2S", "D2S", "S2S", "", "", "", ""},/*QZSS*/
	  {},/*SBS*/
	  {"C1I", "L1I", "D1I", "S1I", "C7I", "L7I", "D7I", "S7I", "", "", "", ""},/*CMP*/
	  {}
  };
  double ver = 3.02;
  set_index(ver, SYS_GPS, opt, tobs[0], index);
  set_index(ver, SYS_GLO, opt, tobs[1], index + 1);
  set_index(ver, SYS_GAL, opt, tobs[2], index + 2);
  set_index(ver, SYS_QZS, opt, tobs[3], index + 3);
  set_index(ver, SYS_SBS, opt, tobs[4], index + 4);
  set_index(ver, SYS_CMP, opt, tobs[5], index + 5);
  set_index(ver, SYS_IRN, opt, tobs[6], index + 6);
  auto nav = &nav_;
  nav->eph = NULL;
  nav->n = nav->nmax = 0;
  nav->geph = NULL;
  nav->ng = nav->ngmax = 0;
  nav->seph = NULL;
  nav->ns = nav->nsmax = 0;
}
bool RealTimeSpp::GetSppPosition(Position *position) {
  int i, n;
  auto popt = &prcopt_;
  for (i = n = 0; i < nobs; i++) {
	if ((satsys(obsds[i].sat, NULL) & popt->navsys) &&
		popt->exsats[obsds[i].sat - 1] != 1)
	  obsds[n++] = obsds[i];
  }
  nobs = 0;
  if (rtkpos(&rtk, obsds, n, &nav_) > 0) {
	double P[9], Q[9];
	double lla[3];
	ecef2pos(rtk.sol.rr, lla);
	soltocov(&(rtk.sol), P);
	covenu(lla, P, Q);
	position->lat = lla[0] * R2D;
	position->lon = lla[1] * R2D;
	lla[2] -= geoidh(lla);
	position->height = lla[2];
	position->std[0] = (Q[4]) < 0 ? 0.0 : sqrt(Q[4]);
	position->std[1] = (Q[0]) < 0 ? 0.0 : sqrt(Q[0]);
	position->std[2] = (Q[8]) < 0 ? 0.0 : sqrt(Q[8]);
	position->q = rtk.sol.stat;
	position->ns = rtk.sol.ns;
	return true;
  } else
	return false;
}

